﻿using PropertyChanged;
using System.Collections.Concurrent;
using System.ComponentModel;
using TPXSDK.NET.GIS;
using TPXSDK.NET.Link;
using TPXSDK.NET.Mission;
using TPXSDK.NET.Share;
using TPXSDK.NET.UObject;

namespace TPXSDK.NET.UAV;

/// <summary>
/// 飞机对象类
/// </summary>

public class UAVItem : INotifyPropertyChanged
{
    /// <summary>
    /// 属性变更通知事件
    /// </summary>
#pragma warning disable 0067
    public event PropertyChangedEventHandler? PropertyChanged;
#pragma warning restore

    /// <summary>
    /// Initializes a new instance of the <see cref="UAVItem"/> class.
    /// </summary>
    public UAVItem(string name, UDevType devType)
    {
        Name = name;
        UavType = devType;
    }

    #region 动态属性
    /// <summary>
    /// 属性字典表(线程安全)
    /// </summary>
    private readonly ConcurrentDictionary<string, object> _propertyDict = new();

    /// <summary>
    /// 根据名称获得指定属性,如指定属性尚未存在则自动注册该属性
    /// </summary>
    public Property<T> Property<T>(string? name = null)
    {
        if (string.IsNullOrEmpty(name))
        {
            name = typeof(T).Name;
        }

        if (_propertyDict.ContainsKey(name) == false)
        {
            RegProperty<T>(name);
        }
        return (Property<T>)_propertyDict[name];
    }

    /// <summary>
    /// 属性注册
    /// </summary>
    public bool RegProperty<T>(string? name = null)
    {
        if (string.IsNullOrEmpty(name))
        {
            name = typeof(T).Name;
        }

        if (_propertyDict.ContainsKey(name))
        {
            throw new Exception($"Duplicate Property:{name}");
        }
        var property = new Property<T>(name);
        return _propertyDict.TryAdd(name, property);
    }

    /// <summary>
    /// 检测属性是否存在
    /// </summary>
    public bool HasProperty(string name)
    {
        return _propertyDict.ContainsKey(name);
    }

    /// <summary>
    /// 获取指定属性的信息
    /// </summary>
    public string? PropertyInfo(string name)
    {
        return !_propertyDict.TryGetValue(name, out object? value)
            ? $"Unknown Property:{name}"
            : value.ToString();
    }
    #endregion

    #region 基本信息

    /// <summary>
    /// 飞行器UAV类型
    /// </summary>
    public UDevType UavType { get; private set; } = UDevType.UNKNOWN;

    /// <summary>
    /// UObjAdmin属性(与UAV Type相关)
    /// </summary>
    [DoNotNotify]
    public UObjAdmin? Admin { get; set; }

    /// <summary>
    /// 飞行器名称
    /// </summary>
    [DoNotNotify]
    public string Name { get; set; }

    /// <summary>
    /// 设备硬件唯一标识编码
    /// </summary>
    [DoNotNotify]
    public uint UID { get; set; }

    /// <summary>
    /// 设备地址,初始值0,可静态设置也可动态分配(用于集群)
    /// </summary>
    [DoNotNotify]
    public uint DevID { get; set; }
    #endregion

    #region 通讯接口
    private TPLink? _link;
    /// <summary>
    /// 数据链通信使用接口Link
    /// </summary>
    public TPLink? Link
    {
        get { return _link; }
        set
        {
            if (_link == value) return;
            if (null != _link)
            {
                _link.TlmRcvEvent -= OnTlmReceived;
            }
            _link = value;
            if (null != _link)
            {
                _link.TlmRcvEvent += OnTlmReceived;
            }
        }
    }

    private void OnTlmReceived(TPLink lnk, TlmType tlmType)
    {
        switch (tlmType)
        {
            case TlmType.STD0: UpdateTlm0(lnk.Tlm0); break;
            case TlmType.STD1: UpdateTlm1(lnk.Tlm1); break;
            case TlmType.STD2: UpdateTlm2(lnk.Tlm2); break;
            case TlmType.STD3: UpdateTlm3(lnk.Tlm3); break;
            case TlmType.STD4: UpdateTlm4(lnk.Tlm4); break;

            case TlmType.GIMBAL: UpdateGimbal(lnk.TlmGimbal); break;
            case TlmType.OBST: UpdateObstacle(lnk.OBST); break;
            case TlmType.TXT: UpdateTlmText(lnk.TlmTxt); break;
            case TlmType.CODE: UpdateTlmCode(lnk.TlmCode); break;
            default: return;
        }
    }

    private void UpdateTlmCode(ushort tlmCode)
    {
        //throw new NotImplementedException();
    }

    private void UpdateTlmText(string? tlmTxt)
    {
        if (string.IsNullOrEmpty(tlmTxt)) return;
        if (tlmTxt.Contains("[SYS]:FTIM"))
        {
            var strVal = tlmTxt[10..];
            if (uint.TryParse(strVal, out var time))
            {
                FlightTime = time;
            }
        }
    }
    #endregion

    #region 位置信息
    /// <summary>
    /// 当前经纬高信息
    /// </summary>
    public AbsPos? Pos { get; set; }

    /// <summary>
    /// HOME点位置
    /// </summary>
    public AbsPos? HomeLoc { get; set; }
    public double HomeDist { get; set; }
    public double HomeBearing { get; set; }

    /// <summary>
    /// 引导点位置
    /// </summary>
    public AbsPos? GuideLoc { get; set; }

    /// <summary>
    /// 断点位置
    /// </summary>
    public AbsPos? BreakPos { get; set; }

    /// <summary>
    /// 盘旋点半径（米）
    /// </summary>
    public int CircleRadius { get; set; }

    #endregion

    #region 控制信息
    /// <summary>
    /// 当前飞行主模式
    /// </summary>
    public int FlightMode { get; set; }
    /// <summary>
    /// 当前飞行子模式
    /// </summary>
    public int FlightStage { get; set; }
    /// <summary>
    /// 目标滚转角，单位度
    /// </summary>
    public float NavRoll { get; set; }
    /// <summary>
    /// 目标俯仰角，单位度
    /// </summary>
    public float NavPitch { get; set; }
    /// <summary>
    /// 目标航向角，单位度
    /// </summary>
    public float NavYaw { get; set; }
    /// <summary>
    /// 目标航迹角，单位度
    /// </summary>
    public float NavCourse { get; set; }
    /// <summary>
    /// 目标高度，单位米
    /// </summary>
    public float NavAlt { get; set; }

    /// <summary>
    /// 目标速度，单位m/s
    /// </summary>
    public float NavSpd { get; set; }
    /// <summary>
    /// 爬升状态标记:爬升/平飞/下降
    /// </summary>
    public int ClimbState { get; set; }
    /// <summary>
    /// 高度变化速率，m/s [升降速率/垂向速度]
    /// </summary>
    public float AltDot { get; set; }
    /// <summary>
    /// 当前飞行速度，m/s
    /// </summary>
    public float Speed { get; set; }
    #endregion

    #region 舵机系统
    public int Aileron { get; set; }
    public int Elevator { get; set; }
    public int Rudder { get; set; }
    public int Throttle { get; set; }
    public int Flap { get; set; }
    public int[]? Servo { get; set; }
    #endregion

    #region 遥控信息
    public int RCRoll { get; set; }  // -100~+100
    public int RCPitch { get; set; } // -100~+100
    public int RCYaw { get; set; }   // -100~+100
    public int RCThr { get; set; }   // -100~+100
    public RCState RCSta { get; set; } = RCState.DIS_CNNT;
    #endregion

    #region 统计信息
    /// <summary>
    /// 当前架次飞行时间，单位秒
    /// </summary>
    public uint FlightTime { get; set; }
    /// <summary>
    /// 总航程,单位米
    /// </summary>
    public uint Voyage { get; set; }
    /// <summary>
    /// 距离HOME点长度,单位米
    /// </summary>
    public uint DistToHome { get; set; }
    /// <summary>
    /// 距离目标点长度,单位米
    /// </summary>
    public uint DistToTarget { get; set; }
    #endregion

    #region 姿态航向
    /// <summary>
    /// 滚转角 deg
    /// </summary>
    public float Roll { get; set; }
    /// <summary>
    /// 俯仰角 deg
    /// </summary>
    public float Pitch { get; set; }
    /// <summary>
    /// 偏航角 deg (机头指向)
    /// </summary>
    public float Yaw { get; set; }
    /// <summary>
    /// 航迹角 deg (运动方向)
    /// </summary>
    public float Course { get; set; }
    #endregion

    #region 高度信息
    /// <summary>
    /// 相对高度 meter（相对Home点）
    /// </summary>
    public float Alt { get; set; }
    /// <summary>
    /// 对地高度 meter
    /// </summary>
    public float AGL { get; set; }
    /// <summary>
    /// GPS高度 meter
    /// </summary>
    public float GPSAlt { get; set; }
    /// <summary>
    /// 气压高度 meter
    /// </summary>
    public float BaroAlt { get; set; }
    #endregion

    #region 任务系统
    /// <summary>
    /// 任务指令列表
    /// </summary>
    public List<FlyCmdItem> FlyCmdList { get; set; } = [];

    /// <summary>
    /// 当前指令索引
    /// </summary>
    public int CmdStep { get; set; } = -1;

    /// <summary>
    /// 任务脚本代码
    /// </summary>
    public string Mission { get; set; } = string.Empty;

    #endregion

    #region GNSS信息

    /// <summary>
    /// GPS1卫星颗数
    /// </summary>
    public int GPS1SatCnt { get; set; }

    /// <summary>
    /// GPS1卫星状态
    /// </summary>
    public byte GPS1Status { get; set; }

    /// <summary>
    /// GPS1位置精度因子
    /// </summary>
    public float GPS1PDOP { get; set; }

    /// <summary>
    /// GPS2卫星颗数
    /// </summary>
    public int GPS2SatCnt { get; set; }

    /// <summary>
    /// GPS2卫星状态
    /// </summary>
    public byte GPS2Status { get; set; }

    /// <summary>
    /// GPS2位置精度因子
    /// </summary>
    public float GPS2PDOP { get; set; }

    /// <summary>
    /// 当前使用的GPS编号(默认0)
    /// </summary>
    public int BestGPS { get; set; }

    /// <summary>
    /// GPS速度（地速 m/s）
    /// </summary>
    public float GndSpeed { get; set; }

    /// <summary>
    /// GPS假锁定状态
    /// </summary>
    public bool IsFakeGPS { get; set; }

    #endregion

    #region 系统状态
    /// <summary>
    /// 执行器是否锁定
    /// </summary>
    public bool IsLocked { get; set; }

    /// <summary>
    /// 是否离地
    /// </summary>
    public bool IsOffGnd { get; set; }

    /// <summary>
    /// 飞控系统是否就绪
    /// </summary>
    public bool IsReady { get; set; }

    /// <summary>
    /// 振动是否告警
    /// </summary>
    public bool IsVibeWarn { get; set; }

    /// <summary>
    /// 日志系统是否正常
    /// </summary>
    public bool IsLogOk { get; set; }

    /// <summary>
    /// 是否已经建立连接
    /// </summary>
    public bool IsConnected { get; set; } = false;

    /// <summary>
    /// 重新联机尝试次数
    /// </summary>
    public int ReCnntCount { get; set; }

    /// <summary>
    /// 联机持续时间
    /// </summary>
    public TimeSpan CnntTimeSpan { get; set; } = new();

    /// <summary>
    /// 系统温度:℃
    /// </summary>
    public float Temp { get; set; }
    /// <summary>
    /// 致命错误编码
    /// </summary>
    public int FatalError { get; set; }
    /// <summary>
    /// 风速:m/s
    /// </summary>
    public float WindSpd { get; set; }
    /// <summary>
    /// 风向:0~360°
    /// </summary>
    public float WindDir { get; set; }
    /// <summary>
    /// 电池低电压标志
    /// </summary>
    public bool IsBatFail { get; set; }
    /// <summary>
    /// 电池1电压
    /// </summary>
    public float Bat1Volt { get; set; }
    /// <summary>
    /// 电池2电压
    /// </summary>
    public float Bat2Volt { get; set; }
    /// <summary>
    /// 是否回放模式
    /// </summary>
    public bool IsReplayMode { get; set; }
    /// <summary>
    /// 遥测字符串报文
    /// </summary>
    public string? TlmText { get; set; }
    /// <summary>
    /// 异步状态编码
    /// </summary>
    public ushort TlmCode { get; set; }
    /// <summary>
    /// 是否使用视觉导航系统
    /// </summary>
    public bool IsVNSUsed { get; set; }
    /// <summary>
    /// 视觉导航系统是否就绪
    /// </summary>
    public bool IsVNSRdy { get; set; }
    /// <summary>
    /// 是否正在进行避障处置
    /// </summary>
    public bool IsAvoiding{ get; set; }
    #endregion

    #region 吊舱载荷
    /// <summary>
    /// Gimbal ENC roll angle in deg
    /// </summary>
    public float GimbalRoll { get; set; }
    /// <summary>
    /// Gimbal ENC pitch angle in deg
    /// </summary>
    public float GimbalPitch { get; set; }
    /// <summary>
    /// Gimbal ENC yaw angle in deg
    /// </summary>
    public float GimbalYaw { get; set; }
    /// <summary>
    /// 激光测距 距离(米)
    /// </summary>
    public float GimbalLaserDist { get; set; }
    /// <summary>
    /// 目标指示 纬度
    /// </summary>
    public double GimbalObjLng { get; set; }
    /// <summary>
    /// 目标指示 经度
    /// </summary>
    public double GimbalObjLat { get; set; }
    /// <summary>
    /// 变倍倍数
    /// </summary>
    public float GimbalZoomLevel { get; set; }
    #endregion

    #region 避障信息
    /// <summary>
    /// 障碍物信息
    /// </summary>
    public List<ObstacleItem> Obsts { get; set; } = [];

    /// <summary>
    /// 障碍物更新时间戳
    /// </summary>
    public DateTime ObsTimeStamp { get; set; } = DateTime.MinValue;
    /// </summary>
    #endregion

    #region 标准遥测
    public void UpdateTlm0(STD_TLM_0 tlm)
    {
        Pitch = tlm.Pitch * 0.01f;
        Roll = tlm.Roll * 0.01f;
        Pos = new AbsPos(tlm.Lng / 1e7, tlm.Lat / 1e7, GPSAlt);

        // 更新至Home点的距离和角度
        if (!AbsPos.IsNullOrEmpty(HomeLoc) && null != HomeLoc)
        {
            HomeDist = Pos.Distance(HomeLoc);
            if (HomeDist > 0.5f) HomeBearing = Pos.Bearing(HomeLoc);
        }

        Speed = tlm.Speed * 0.01f;
        NavSpd = tlm.TargetSpd * 0.01f;
        RCSta = (RCState)tlm.RCState;
        FlightMode = tlm.FlightMode >> 4;
        FlightStage = tlm.FlightMode & 0x0F;

        BaroAlt = tlm.BaroAlt * 0.01f;
        AGL = tlm.AGL * 0.01f;// 转换为m

        // 电池电压[V]
        Bat1Volt = 0.01f * tlm.Volt1;
        Bat2Volt = 0.01f * tlm.Volt2;
    }

    public void UpdateTlm1(STD_TLM_1 tlm)
    {
        Pitch = tlm.Pitch * 0.01f;
        Roll = tlm.Roll * 0.01f;
        Pos = new AbsPos(tlm.Lng / 1e7, tlm.Lat / 1e7, GPSAlt);
        // 更新至Home点的距离和角度
        if (!AbsPos.IsNullOrEmpty(HomeLoc) && null != HomeLoc)
        {
            HomeDist = Pos.Distance(HomeLoc);
            if (HomeDist > 1.0f) HomeBearing = Pos.Bearing(HomeLoc);
        }

        BestGPS = tlm.BestGPS;
        GPS1SatCnt = tlm.GPS1SatNum;
        GPS1Status = tlm.GPS1Status;
        GPS1PDOP = tlm.GPS1DOP * 0.01f;

        GPS2SatCnt = tlm.GPS2SatNum;
        GPS2Status = tlm.GPS2Status;
        GPS2PDOP = tlm.GPS2DOP * 0.01f;

        GndSpeed = tlm.GndSpd * 0.01f;  // 地速:m/s
        GPSAlt = tlm.GPSAlt * 0.01f;    // 海拔:meter
        WindDir = tlm.WindDir * 360.0f / 256.0f;// 风向:0~360°
        WindSpd = tlm.WindSpd * 0.01f;  // 风速:m/s
    }

    public void UpdateTlm2(STD_TLM_2 tlm)
    {
        Pitch = tlm.Pitch * 0.01f;
        Roll = tlm.Roll * 0.01f;
        Pos = new AbsPos(tlm.Lng / 1e7, tlm.Lat / 1e7, GPSAlt);
        // 更新至Home点的距离和角度
        if (!AbsPos.IsNullOrEmpty(HomeLoc) && null != HomeLoc)
        {
            HomeDist = Pos.Distance(HomeLoc);
            if (HomeDist > 1.0f) HomeBearing = Pos.Bearing(HomeLoc);
        }

        // 伺服输出, 范围 0~200，转换为 1000~2000 us
        Servo =
        [
            1000 + 5 * tlm.SRV1,
            1000 + 5 * tlm.SRV2,
            1000 + 5 * tlm.SRV3,
            1000 + 5 * tlm.SRV4,
            1000 + 5 * tlm.SRV5,
            1000 + 5 * tlm.SRV6,
            1000 + 5 * tlm.SRV7,
            1000 + 5 * tlm.SRV8
        ];

        // RC遥控器输出, 范围 -100~100
        RCRoll = tlm.RCR;
        RCPitch = tlm.RCP;
        RCYaw = tlm.RCY;
        RCThr = tlm.RCT;

        // 电池电流信息
        //Bat1Cur = tlm.Cur1;
        //Bat2Cur = tlm.Cur2;
    }

    public void UpdateTlm3(STD_TLM_3 tlm)
    {
        Pitch = tlm.Pitch * 0.01f;
        Roll = tlm.Roll * 0.01f;
        Pos = new AbsPos(tlm.Lng / 1e7, tlm.Lat / 1e7, GPSAlt);
        // 更新至Home点的距离和角度
        if (!AbsPos.IsNullOrEmpty(HomeLoc) && null != HomeLoc)
        {
            HomeDist = Pos.Distance(HomeLoc);
            if (HomeDist > 1.0f) HomeBearing = Pos.Bearing(HomeLoc);
        }

        Yaw = tlm.Yaw * 0.01f;
        NavYaw = tlm.TargetYaw * 0.01f;

        Course = tlm.Hdg * 0.01f;
        NavCourse = tlm.TargetHdg * 0.01f;
        NavAlt = tlm.TargetAlt * 0.01f;

        Aileron = tlm.Aileron;
        Elevator = tlm.Elevator;
        Rudder = tlm.Rudder;
        Throttle = tlm.Throttle;
        CmdStep = tlm.CmdStep;
    }

    public void UpdateTlm4(STD_TLM_4 tlm)
    {
        Pitch = tlm.Pitch * 0.01f;
        Roll = tlm.Roll * 0.01f;
        Pos = new AbsPos(tlm.Lng / 1e7, tlm.Lat / 1e7, GPSAlt);
        // 更新至Home点的距离和角度
        if (!AbsPos.IsNullOrEmpty(HomeLoc) && null != HomeLoc)
        {
            HomeDist = Pos.Distance(HomeLoc);
            if (HomeDist > 1.0f) HomeBearing = Pos.Bearing(HomeLoc);
        }

        NavPitch = tlm.TargetPitch * 0.01f;
        NavRoll = tlm.TargetRoll * 0.01f;
        Alt = tlm.Alt * 0.01f;
        AltDot = tlm.AltDot * 0.01f;

        Temp = tlm.Temp;
        ClimbState = tlm.ClimbState;
        Voyage = tlm.Voyage;// 总航程

        // 状态标记位
        IsReady = DataUtils.BIT16_TST(tlm.Flags, 0);
        IsOffGnd = DataUtils.BIT16_TST(tlm.Flags, 1);
        IsLocked = DataUtils.BIT16_TST(tlm.Flags, 2);
        IsVibeWarn = DataUtils.BIT16_TST(tlm.Flags, 3);
        IsLogOk = DataUtils.BIT16_TST(tlm.Flags, 4);
        IsBatFail = DataUtils.BIT16_TST(tlm.Flags, 5);
        IsAvoiding = DataUtils.BIT16_TST(tlm.Flags, 6);
        IsVNSUsed = DataUtils.BIT16_TST(tlm.Flags, 7);
        IsVNSRdy  = DataUtils.BIT16_TST(tlm.Flags, 8);
    }

    /// <summary>
    /// 更新吊舱遥测报文信息
    /// </summary>
    public void UpdateGimbal(GIMBAL_TLM gimbal)
    {
        GimbalRoll = (float)(0.01f * gimbal.Roll);
        GimbalPitch = (float)(0.01f * gimbal.Pitch);
        GimbalYaw = (float)(0.01f * gimbal.Yaw);
        GimbalZoomLevel = (float)(0.1f * gimbal.Zoom);
        GimbalLaserDist = (float)(0.01f * gimbal.Dist);
        GimbalObjLng = 1e-7 * gimbal.Lng;
        GimbalObjLat = 1e-7 * gimbal.Lat;
    }

    /// <summary>
    /// 更新障碍物信息
    /// </summary>
    public void UpdateObstacle(byte[]? obst)
    {
        Obsts.Clear();
        if (null != obst)
        {
            var cnt = obst[0];
            var oft = 1;
            for (int i = 0; i < cnt; i++)
            {
                var item = new ObstacleItem
                {
                    Distance = BitConverter.ToSingle(obst, oft)
                };
                oft += 4;
                item.Azimuth = BitConverter.ToSingle(obst, oft);
                oft += 4;
                Obsts.Add(item);
            }
        }
        ObsTimeStamp = DateTime.Now;
    }

    /// <summary>
    /// 更新广播报文信息(集群)
    /// </summary>
    public void UpdateBroadcast(BCST_TLM tlm)
    {
        //FIXME
    }
    #endregion

    #region 默认路径
    public string? TlmPath { get; set; }
    public string? SettingPath { get; set; }
    public string? LogPath { get; set; }
    public string? MissionPath { get; set; }
    #endregion
}
